/*
 
Copyright Eric C. McCreath 2012  <ericm@cs.anu.edu.au>

This file is part of the clapper-webcam-recorder project.

clapper-webcam-recorder is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License,
or (at your option) any later version.

clapper-webcam-recorder is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with clapper-webcam-recorder.  If not, see <http://www.gnu.org/licenses/>.

  */



#include <stdio.h>
#include <string.h>

#include <GL/glut.h>
#include <GL/glu.h>
#include <linux/videodev2.h>
#include <pthread.h>
#include <semaphore.h>
#include <sched.h>

#include "display.h"
#include "webcam.h"
#include "savevideo.h"
#include "sound.h"
#include "clapper.h"
#include "util.h"
#include "x11display.h"


int exitsequence = 0; // 0 - no exit, 
                      // 1 - start exit eos video frame move to 2, end video capture, and post svd->audiogo
                      // 2 - eos audio move to 3 
                      // 3 - stop capture loop - close stream  - exit program
                      

int webcam = 1;
int mixcam = 0;
int xoffset, yoffset;
int warmupgo = 0; 
int doshow = 0;

int lside = 100;
int tside = 60;
int gap = 20;
int consumeextra = 0;

struct savevideodata *svd;
struct x11displayinfo *di;


pthread_t capturevideothread;
pthread_t captureaudiothread;
pthread_t warmupthread;


int displayscale = 2;

int ydatasize; 
int udatasize; 
int vdatasize;


#define CLAPBUFSIZE 7
#define CLAPMPOS 3
int currclappos;
unsigned char *clapbuffery[CLAPBUFSIZE];
unsigned char *clapbufferu[CLAPBUFSIZE];
unsigned char *clapbufferv[CLAPBUFSIZE];



#define MYBUFSIZE 20
unsigned char *myvidbuffery[MYBUFSIZE];
unsigned char *myvidbufferu[MYBUFSIZE];
unsigned char *myvidbufferv[MYBUFSIZE];
double myvidcapturetime[MYBUFSIZE];
double myvidfirsttime;
int myvidstart;
int myvidsize;
int myvidframe;
int myvidblocksize;
sem_t myvidmutex, myvidempty;

void 
initmybuf(int blocksize) {
    int i;
    myvidsize = 0;
    myvidstart = 0;
    myvidframe = 0;
    myvidblocksize = blocksize; // in bytes
    for (i=0; i<MYBUFSIZE;i++) {
        myvidbuffery[i] = malloc(ydatasize); 
        myvidbufferu[i] = malloc(udatasize); 
        myvidbufferv[i] = malloc(vdatasize); 
    }
    sem_init(&myvidmutex, 0,1);
    sem_init(&myvidempty, 0,0);

     for (i=0; i<CLAPBUFSIZE;i++) {
        clapbuffery[i] = malloc(ydatasize); 
        clapbufferu[i] = malloc(udatasize); 
        clapbufferv[i] = malloc(vdatasize); 
    }

}

double
getcapturetime() {
    return myvidcapturetime[myvidstart];
}


static void *
capturevideointomybuf(void *arg) {
  
   struct v4l2_buffer buf;
   XImage *image;
   double expectedtime;
   double captime, scaptime;
   
   double missedby; 
   int putpos;
   int capturewebcam;
   int capturescreen;
   int firsttime = 1;
   struct timespec nst;
   nst.tv_sec = 0;
   nst.tv_nsec = (long) ((1.0 / svd->framerate) * 1000000000.0);
   int recapture = 0;

   sem_wait (&(svd->videogo));

   while(exitsequence < 3) {
          
           //printf("capture\n");
           capturewebcam = 0;
           capturescreen = 0;
           if (webcam) {
              capturewebcam = 1;
           } else {
              capturescreen = 1;
           }

           expectedtime = (myvidframe * 1.0) / svd->framerate;
           if (capturewebcam) {
               do {
                   getbuffer_webcam(&buf);
               
                              
                   captime = capturetime(&buf);
                   if (firsttime) {
                      myvidfirsttime = captime;
                      firsttime = 0;
                   }
                   missedby = expectedtime - (captime-myvidfirsttime);
//printf("bf %.3f mb %.3f \n",(-0.5*(1.0 / svd->framerate)),missedby);
 
                   recapture = missedby >  (0.5*(1.0 / svd->framerate));
                   if (recapture) {
                       releasebuffer_webcam(&buf);
                   }
              } while (recapture);
                   
                 
//printf("%.6f %.6f\n",captime,expectedtime);
           }
           if (capturescreen) {
                scaptime = timefromstart();
                 if (!capturewebcam && firsttime) {
                       myvidfirsttime = scaptime;
                       firsttime = 0;
                 }
                 if (!capturewebcam) {
                    while ((timefromstart()-myvidfirsttime)<expectedtime) nanosleep(&nst,NULL);
                 }
                 image = grabimage(di, xoffset, yoffset ,svd->xdims, svd->ydims);
                
                 if (!capturewebcam) {
                       captime = scaptime;
                 }
           }
           missedby = expectedtime - (captime-myvidfirsttime);
          
//printf("et %.3f ct-mft %.3f mb %.3f \n",expectedtime, (captime-myvidfirsttime),missedby);
          

           sem_wait (&myvidmutex);
           if (myvidsize == MYBUFSIZE) s1_exit("myvid buffer full\n");
           putpos = (myvidstart + myvidsize) % MYBUFSIZE;
           
              
           
           sem_post (&myvidmutex);

           myvidcapturetime[putpos] = captime;
           if (capturewebcam) { 
               if (doshow) updateTexture(buffers[buf.index].start);
               savetobuffer(svd, putpos, buffers[buf.index].start);
               releasebuffer_webcam(&buf);
           } 
           if (capturescreen) {
                savetobufferImageAndTexture(svd, putpos, image);
                XDestroyImage(image);
           }

           sem_wait (&myvidmutex);
           myvidsize++;
           sem_post (&myvidmutex);
           sem_post(&myvidempty);


           myvidframe++;
   }
  

   return NULL;
}


void updateTexture(const unsigned char *p)
{
    int x,y;
    int ydimd = ydim/displayscale;
    int xdimd = xdim/displayscale;
    int pos;

    if (displayscale == 1) {
        for (y = 0; y < ydim; y++) {    
            for (x = 0; x < xdim; x += 2) { 
                setcolortexbuf(x,y,p[x*2 + y*640*2], 
                                   p[x*2 + 1 + y*640*2], 
                                   p[x*2 + 3 + y*640*2]);
                setcolortexbuf(x+1,y,p[(x+1)*2 + y*640*2],
                                     p[(x+1)*2 - 1 + y*640*2],
                                     p[(x+1)*2 + 1 + y*640*2]); 
            }
        }    
    } else {
        for (y = 0; y < ydimd; y++) {    
            for (x = 0; x < xdimd; x++) { 
                pos = x*displayscale*2 + y*displayscale*640*2;
                setcolortexbuf(x,y,p[pos], 
                                   p[pos + 1], 
                                   p[pos + 3]);
            }
        }  
    }
}


void keypress (unsigned char key, int x, int y) {
   if (key == 'q') {
       exitsequence = 1;
   } else if (key == '1') {
       webcam = 1;
   } else if (key == '2') {
       xoffset = lside;
       yoffset = tside;
       webcam = 0;
   } else if (key == '3') {
       xoffset = lside + xdim + gap;
       yoffset = tside;
       webcam = 0;
   } else if (key == '4') {
       xoffset = lside;
       yoffset = tside + ydim + gap;
       webcam = 0;
   } else if (key == '5') {
       xoffset = lside + xdim + gap;
       yoffset = tside + ydim + gap;
       webcam = 0;
   } else if (key == '6') {
       mixcam = !mixcam;
   } else if (key == 'z') {
      currclappos--;
      if (currclappos < 0) currclappos = 0;
      drawtotexture(clapbuffery[currclappos],clapbufferu[currclappos],clapbufferv[currclappos]);
      drawboxtotexture(15 + 25*currclappos, 20, 20, (currclappos == CLAPMPOS ? 0xFFFF00 : 0xFFFFFF));
   } else if (key == 'x') {
      currclappos++;
      if (currclappos >= CLAPBUFSIZE) currclappos = CLAPBUFSIZE - 1;
      drawtotexture(clapbuffery[currclappos],clapbufferu[currclappos],clapbufferv[currclappos]);
      drawboxtotexture(15 + 25*currclappos, 20, 20, (currclappos == CLAPMPOS ? 0xFFFF00 : 0xFFFFFF));
   } else if (key == 'c') {
      printf("continue clapping\n");
      consumeextra =  currclappos - CLAPMPOS;
   } else if (key == 's') {
      printf("start recording\n");
      consumeextra =  currclappos - CLAPMPOS;
      warmupgo = 0;
   }
}


void display(void) {
    drawtexture();
    glFlush(); 
    glutSwapBuffers();

}

void endProgram() {
    printf("End Program\n");

    pthread_join( capturevideothread, NULL);

    stop_webcam();
    close_webcam();
    closepulse();
    closetheora(svd);
    closevorbis(svd);
    closeogg(svd);
    exit(0);
} 

void timer() {
   
    glutPostRedisplay();

    if (exitsequence < 2) {
        glutTimerFunc(100,timer,0);
    } 
}

void savetobufferImageAndTexture(struct savevideodata *sd, int bi, XImage *image) {
    int x,y, xh, yh; 
    int r,g,b;
    int yy,u,v;
    int pix;
     
    for(y = 0; y < sd->ydims; y++) {
        yh = y / 2;
        for(x = 0; x < sd->xdims; x++) {
            pix = XGetPixel(image,x,y);   
            r = (pix>>16) & 0xFF;   // conversion based on wikipedia
            g = (pix>>8) & 0xFF;
            b = pix & 0xFF;

            yy = (((66*r + 129*g + 25*b) + 128)>>8) + 16;
            u = (((-38*r - 74*g + 112*b) + 128)>>8) + 128;
            v = (((112*r -94*g - 18*b) + 128)>>8) + 128;

            (myvidbuffery[bi])[x + y * sd->yuv_w] =  yy & 0xFF;

            if (y%2 == 0 && x%2 == 0) {
                xh = x / 2;
                (myvidbufferu[bi])[xh + yh * (sd->xdims>>1)] =  u  & 0xFF;
                (myvidbufferv[bi])[xh + yh * (sd->xdims>>1)] =  v & 0xFF;
            }
            if (doshow && ((x%displayscale) == 0) && ((y%displayscale) == 0)) 
                setcolor(x/displayscale,y/displayscale,pix);
        }
    }
}

double intensity(struct savevideodata *sd,void * p) {
    unsigned char *pp;
    int x,y; 
    pp = (unsigned char *) p;
    double sum = 0.0;
    for(y = 0; y < sd->ydims; y++) {
        for(x = 0; x < sd->xdims; x++) {
            sum +=  (pp[2*x + y * 2*sd->yuv_w]) / 255.0;
        }
    }
    return sum/(sd->ydims* sd->xdims);
}

void savetobuffer(struct savevideodata *sd, int bi, void * p) {
    char *pp;
    int x,y; 
    pp = (char *) p;

    for(y = 0; y < sd->ydims; y++) {
        for(x = 0; x < sd->xdims; x++) {
            (myvidbuffery[bi])[x + y * sd->yuv_w] =  pp[2*x + y * 2*sd->yuv_w];

        }
    }

    for(y = 0; y < sd->ydims>>1; y++) {
        for(x = 0; x < sd->xdims>>1; x++) {
            (myvidbufferu[bi])[x + y * (sd->xdims>>1)] =  pp[4*x+1 + y * 4*sd->yuv_w];
            (myvidbufferv[bi])[x + y * (sd->xdims>>1)] =  pp[4*x+3 + y * 4*sd->yuv_w];
        }
    }
}


static void *
captureaudio(void *arg) {
    sem_wait (&(svd->audiogo));
    saveoggvorbis(svd);


   return NULL;
}

double 
saveframe(int last) {
    int getpos;
    double time;

    sem_wait (&myvidempty);
    getpos = myvidstart;

    (svd->ycbcr)[YPOS].data = myvidbuffery[getpos]; 
    (svd->ycbcr)[UPOS].data = myvidbufferu[getpos]; 
    (svd->ycbcr)[VPOS].data = myvidbufferv[getpos];
    time = myvidcapturetime[getpos];

    sem_wait (&myvidmutex);
    myvidsize--;
    myvidstart = (myvidstart + 1) % MYBUFSIZE;
    sem_post (&myvidmutex);
        
    saveoggframe(svd,  last);
    svd->granulepause = svd->samplesperframe * svd->frame;
    return time;
   
}


void drawtotexture(unsigned char * yd, unsigned char *ud, unsigned char *vd) {
    int x,y;
    int ydimd = ydim/displayscale;
    int xdimd = xdim/displayscale;
    int ypos, upos, vpos;

    for (y = 0; y < ydimd; y++) {    
        for (x = 0; x < xdimd; x++) { 
                ypos = x*displayscale + y*displayscale*svd->yuv_w;
                upos = x*displayscale/4 + y*displayscale*(xdim>>2);
                vpos = x*displayscale/4 + y*displayscale*(xdim>>2);
               setcolortexbuf(x,y,yd[ypos], 
                                   ud[upos], 
                                   vd[vpos]);
        }
    }  
   
}

void drawboxtotexture(int xp, int yp, int size, unsigned long  rgb) {
    int x,y;


    for (y = yp/displayscale; y < (yp+size)/displayscale; y++) {    
        for (x = xp/displayscale; x < (xp+size)/displayscale; x++) { 
              
               setcolor(x,y,rgb);
        }
    }     
}


void 
copyyuv(int d, int s) {
    memcpy(clapbuffery[d],myvidbuffery[s],ydatasize);
    memcpy(clapbufferu[d],myvidbufferu[s],udatasize);
    memcpy(clapbufferv[d],myvidbufferv[s],vdatasize);
}

int
consumeframe(int show, int savepos) {
    int getpos;

    sem_wait (&myvidempty);
    getpos = myvidstart;

    if (show) drawtotexture(myvidbuffery[getpos],myvidbufferu[getpos],myvidbufferv[getpos]);
    if (savepos != -1) copyyuv(savepos,getpos);

    //time = myvidcapturetime[getpos];

    sem_wait (&myvidmutex);
    myvidsize--;
    myvidstart = (myvidstart + 1) % MYBUFSIZE;
    sem_post (&myvidmutex);
    return getpos;
   
}





static char * dev_name;
static char * out_file_name;


void testFrameRate() {
    int i; 
    struct v4l2_buffer buf;
    double starttime, endtime,totaltime;
    int loops = 500;
    XImage *image;
    
    printf("Test frame rate - based on max webcam framerate\n");
    open_webcam(dev_name);
    init_webcam();
    start_webcam();

    starttime = secondsfrom();
    for (i=0;i<loops;i++) {
        getbuffer_webcam(&buf);
        releasebuffer_webcam(&buf);
    }
    endtime = secondsfrom();
    totaltime = endtime - starttime;
    printf("webcam time %.3f max rate %.3f\n", totaltime, (loops*1.0)/totaltime);
    stop_webcam();
    close_webcam();

   di = setupx11display();
   starttime = secondsfrom();
    for (i=0;i<loops;i++) {
      image = grabimage(di,0,0,xdim,ydim);
      XDestroyImage(image);
    }
    endtime = secondsfrom();
    totaltime = endtime - starttime;
    printf("x11 grab time %.3f max rate %.3f image (%d x %d)\n",
             totaltime, (loops*1.0)/totaltime, image->width, image->height);
}
static void *
warmup() {
   // double last = 0.0;
   // double curr;
    
    
    int samplepos = 0;
    int framepos = 0;
    double vol;
    double avg = 0.1;
    int count = 0; 
    double alpha = 0.05;
    int clap = 0;
    int mpos, i;

    sem_wait (&(svd->warmupgo));
    
    //for (i =0;i<10;i++)pulseread();
    while (warmupgo || consumeextra != 0) {

        pulseread();
        samplepos += READ;
        vol = volumn();
        if (vol > avg*10.0) {
               printf("clap count %d myvidsize %d avg %.6f v : %.6f v/avg %.6f\n",count, myvidsize, avg, vol, vol/avg);
               clap = 1;
        }
        //printf("avg %.6f v : %.6f\n",avg, vol);
        avg = alpha*vol + (1.0-alpha)*avg;
        count++;
         
        
        if (samplepos > framepos*svd->samplerate/svd->framerate) {
            if ((clap > 0) && 
                (clap <= (CLAPBUFSIZE - CLAPMPOS))) {
                 mpos = consumeframe(((clap==1)?1:0), CLAPMPOS + clap - 1);  
                 if (clap == 1) {
                     for (i=1; i <= CLAPMPOS; i++) {
                         copyyuv(CLAPMPOS-i,(MYBUFSIZE+mpos-i)%MYBUFSIZE);
                     }
                     currclappos = CLAPMPOS;
                 }
                 clap++;
            } else {
                if (consumeextra == 0) {
                    consumeframe(0,-1);
                } else if (consumeextra < 0) {
                    consumeextra++;
                } else  {
                    consumeframe(0,-1);
                    consumeframe(0,-1);
                    consumeextra--;
                }
                clap = 0;
            }         
            framepos++;
        }
    }
    doshow = 1;
    sem_post (&(svd->audiogo));
    return NULL;
}

static void usage(int argc, char *argv[])
{
    printf("Usage: %s [options] [out file name]\n\n"
           "The default out file name is out.ogv"
           "Options:\n"
           " -d name - Video device name [default /dev/video0]\n"
           " -r rate - set the frame rate [default %d]\n"
           " -t      - test the maximum frame rate\n"
           " -h      - Print this message\n"
           " -w      - Warmup with a clap (experimental)\n"
           "           clap then 'z' and 'x' to move back and forward\n" 
           "           to the clap frame, 's' to start recording,\n" 
           "           'c' to continue clapping.\n"
           " Keys : q - quit, 1 webcam, 2-5 screens\n",  
         argv[0],FRAMERATEDEFAULT);
}


int main(int argc, char **argv) {

    int testframerate = 0;
    int framerate = FRAMERATEDEFAULT;
      

   /* had played with this but don't think it is needed. 
     struct sched_param schedparam; 
     if (sched_setscheduler(0,SCHED_RR, &schedparam)) {
         printf("it would be nice set as a realtime scheduler : either run as root or add the line\n <your username> -  rtprio 100\n to /etc/security/limits.conf and reboot\n");
    }*/

 // process args
    dev_name = "/dev/video0";
    out_file_name = "out.ogv";
    int argpos = 1;
    while (argpos < argc) {
        if (strcmp(argv[argpos],"-h")==0) {
            usage(argc,argv);
            exit (EXIT_SUCCESS);
        } else if (strcmp(argv[argpos],"-t")==0) {
            testframerate = 1;
        } else if (strcmp(argv[argpos],"-w")==0) {
            warmupgo = 1; 
        } else if (strcmp(argv[argpos],"-d")==0) {
            if (argpos+1 == argc) s1_exit("device name expected\n");
            argpos++;
            dev_name = argv[argpos];
        } else if (strcmp(argv[argpos],"-r")==0) {
            if (argpos+1 == argc) s1_exit("framerate expected\n");
            argpos++;
             if (sscanf(argv[argpos], "%d", &framerate) != 1) s1_exit("framerate parse problem");
             if (framerate < 1) s1_exit("framerate should be 1 or greater");
             if (framerate > 100) s1_exit("frame rate is very large???");
        } else if (((argpos + 1) == argc) && (argv[argpos][0] != '-')) {
            out_file_name = argv[argpos];
        } else {
            s2_exit("unknown option : %s\n", argv[argpos]);
        }
        argpos++;
    }

    if (testframerate) { 
        testFrameRate();
    } else {
       glutInit(&argc, argv);
       setupdisplay(xdim/displayscale,ydim/displayscale);
       glutDisplayFunc(display);
       glutTimerFunc(25,timer,0);
       glutKeyboardFunc(keypress);

       open_webcam(dev_name);
       svd = setupogg(out_file_name);
       di = setupx11display();
       setuptheora(svd, xdim, ydim, framerate, &ydatasize, &udatasize, &vdatasize);
       setupvorbis(svd, 44100);
       setuptheora2(svd);
       
       init_webcam();

       initmybuf(buffers[0].length);
      
       setuppulse();

       sem_init(&(svd->videogo), 0,0);     
       sem_init(&(svd->audiogo), 0,0);
       if (warmupgo) sem_init(&(svd->warmupgo),0,0);
      	 
       if (pthread_create( &captureaudiothread, NULL, captureaudio, NULL)) s1_exit("problem creating captureaudionthread");	  

       if (pthread_create( &capturevideothread, NULL, capturevideointomybuf, NULL)) s1_exit("problem creating captureaudionthread");
       if (warmupgo) {
           if (pthread_create( &warmupthread, NULL, warmup, NULL)) s1_exit("problem creating warmupthread");
       }

       start_webcam();

       setstarttime();
       sem_post (&(svd->videogo));
       if (warmupgo) {
           sem_post (&(svd->warmupgo));
       } else {
           doshow = 1;
           sem_post (&(svd->audiogo));
       }
       glutMainLoop();  // this never returns !!
      
      
       
       

       
      
    }
    return 0;
}
